#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#define SKIP fread(&dummy, sizeof(dummy), 1, pf);

int grid(float *x,float *y,float *z, float rlbox, int ngrid, int nobj, 
         int *lirst, int *ll);

int busv(float *x, float *y, float *z, int ic, float rlbox, int ngrid, int nobj,
         int *lirst, int *ll, int *grupo, float r0, int iper, int *listvec, 
         int *nvec);

int iden(float *x, float *y, float *z, float rlbox, float r0, int nobj, 
         int iper, int *grupo);

int main(int argc, char **argv)
{
  struct io_header_1
  {
    int      npart[6];
    double   mass[6];
    double   time;
    double   redshift;
    int      flag_sfr;
    int      flag_feedback;
    int      npartTotal[6];
    int      flag_cooling;
    int      num_files;
    double   BoxSize;
    double   Omega0;
    double   OmegaLambda;
    double   HubbleParam; 
    char     fill[256- 6*4- 6*8- 2*8- 2*4- 6*4- 2*4 - 4*8];
  } header1;
  int   i;
  int   nobj;
  float *x,*y,*z;
  float *vx,*vy,*vz;
  int *Id;
  float rlbox;
  int iper;
  int *grupo;
  int *numero;
  float r0;
  char filename[200];
  FILE  *pf;
  int dummy, n, k, pc;
  float r[3];

  sprintf(filename,"gadget_lineal_z3.dat");
  /*sprintf(filename,"../gadget/Gadget-1.1/P-Gadget/data/snapshotzel_021");*/
  if(!(pf=fopen(filename,"r")))
  {
    printf("can't open file `%s`\n",filename);
    exit(0);
  }
  printf("paso el opn\n");

  fread(&dummy, sizeof(dummy), 1, pf);
  fread(&header1, sizeof(header1), 1, pf);
  fread(&dummy, sizeof(dummy), 1, pf);
  printf("paso el header\n");
  nobj=0;
  for(k=0,pc=0;k<6;k++)
    nobj+=header1.npart[k];
  vx          = malloc(nobj*sizeof(float));
  vy          = malloc(nobj*sizeof(float));
  vz          = malloc(nobj*sizeof(float));
  x          = malloc(nobj*sizeof(float));
  y          = malloc(nobj*sizeof(float));
  z          = malloc(nobj*sizeof(float));
  grupo      = malloc(nobj*sizeof(int));
  numero     = malloc(nobj*sizeof(int));
  Id         = malloc(nobj*sizeof(int));
  SKIP;
  for(k=0,pc=0;k<6;k++)
  {
    for(n=0;n<header1.npart[k];n++)
    {
      fread(&r[0], sizeof(float), 3, pf);
      x[pc]=r[0];
      y[pc]=r[1];
      z[pc]=r[2];
      pc++;
    }
  }
  SKIP;
  SKIP;
  for(k=0,pc=0;k<6;k++)
  {
    for(n=0;n<header1.npart[k];n++)
    {
      fread(&r[0], sizeof(float), 3, pf);
      vx[pc]=r[0];
      vy[pc]=r[1];
      vz[pc]=r[2];
      pc++;
    }
  }
  SKIP;
  SKIP;
  for(k=0,pc=0;k<6;k++)
  {
    for(n=0;n<header1.npart[k];n++)
    {
      fread(&Id[pc], sizeof(int), 1, pf);
      pc++;
    }
  }


  fclose(pf);

  iper=0;
  rlbox = header1.BoxSize;
  r0 = 0.55*rlbox/pow((float)nobj,0.33333);
  printf("nobj rlbox r0 %d %f %f\n",nobj,rlbox,r0);
  
  iden(x,y,z,rlbox,r0,nobj,iper,grupo);

  for(i=0;i<nobj;i++)
    numero[i]=0;
  for(i=0;i<nobj;i++)
    if(grupo[i]>0)numero[grupo[i]]++;

  /*pf=fopen("iden_nolineal_z0.dat","w+");*/
  pf=fopen("iden_lineal_z3.dat","w+");
  for( i = 0 ; i < nobj; i++ )
    if(numero[grupo[i]]>9)fprintf(pf,"%f %f %f %f %f %f %d %d %d\n",x[i],y[i],z[i],vx[i],vy[i],vz[i],Id[i],grupo[i],numero[grupo[i]]);
  fclose(pf);
}


int iden(float *x, float *y, float *z, float rlbox, float r0, int nobj, 
         int iper, int *grupo)
{
/*se asume que las posisiones estan entre 0 y rlbox*/
  int   i, j;
  int   *lirst, *ll;
  int   *listvec, *listvecnew;
  int   ngrid;
  int   nvec, nvecvec;
  int   ngrupos;

  ngrid=(int)(rlbox/r0);
  if(ngrid>256)ngrid=256;
  printf("r0 ngrid %f %d\n",r0,ngrid);
  lirst      = malloc(ngrid*ngrid*ngrid*sizeof(int));
  ll         = malloc(nobj*sizeof(int));
  listvec    = malloc(nobj*sizeof(int));
  listvecnew = malloc(nobj*sizeof(int));

  grid(x,y,z,rlbox,ngrid,nobj,lirst,ll);

/*empieza la identificacion*/
  for( i = 0 ; i < nobj ; i++ )
    grupo[i]=0;
  
  ngrupos = 0;
/*  pf=fopen("identest.dat","w+");*/
  for( i = 0 ; i < nobj; i++ )
  {
    if( grupo[i] != 0 ) continue; /*salta a la siguiente*/
    printf("%d\n",i);
    nvec=0;
    busv(x,y,z,i,rlbox,ngrid,nobj,lirst,ll,grupo,r0,iper,listvec,&nvec);
/*	printf("nvec %f %f %f %d\n",x[i],y[i],z[i],nvec);*/
    if(nvec!=0)
    {
      ngrupos++;
      grupo[i]=ngrupos;
    }
    for( j = 0 ; j < nvec ; j++ )
      grupo[listvec[j]]=ngrupos;
	

    do
    {
      nvecvec=0;
      for( j = 0 ; j < nvec ; j++ )
      {
        int k;
        busv(x,y,z,listvec[j],rlbox,ngrid,nobj,lirst,ll,grupo,r0,iper,
             listvecnew,&nvecvec);
        for(k=0;k<nvecvec;k++)
          grupo[listvecnew[k]]=ngrupos;
      }

      if( nvecvec == 0 ) break;
      for( j = 0 ; j < nvecvec ; j++ )
        listvec[j]=listvecnew[j];
      nvec=nvecvec;
/*      printf("      nvecvec %d\n",nvecvec);*/
/*      for(j=0;j<nvecvec;j++)
      {
        int ind;
        ind=listvecnew[j];
        fprintf(pf,"%f %f %f %d %d\n",x[ind],y[ind],z[ind],ind,grupo[ind]);
      }
      exit(0);*/
    } while( 1 ); /*fin del do de los vecinos de los vecinos*/
  } 
/*  fclose(pf);*/
/*termino la identificacion*/
}

int grid(float *x,float *y,float *z, float rlbox, int ngrid, int nobj, 
         int *lirst, int *ll)
{
  int i, j, k;
  int ix, iy, iz;
  float fac;

  fac = (float)ngrid/rlbox ;

  printf("grid\n");
  for( i = 0 ; i < ngrid ; i++)
    for( j = 0 ; j < ngrid ; j++)
      for( k = 0 ; k < ngrid ; k++)
        lirst[(i * ngrid + j) * ngrid + k] = 0 ;

  for( i = 0 ; i < nobj ; i++ )  
    ll[i] = 0 ;

  for( i = 0 ; i < nobj ; i++ )
  {
    ix=(int)(x[i]*fac);
    iy=(int)(y[i]*fac);
    iz=(int)(z[i]*fac);
    lirst[(ix * ngrid + iy) * ngrid + iz] = i ;
  }  

  for( i = 0 ; i < nobj ; i++ )
  {
    ix=(int)(x[i]*fac);
    iy=(int)(y[i]*fac);
    iz=(int)(z[i]*fac);
    ll[lirst[(ix * ngrid + iy) * ngrid + iz]] = i ;
    lirst[(ix * ngrid + iy) * ngrid + iz] = i ;
  }  

  printf("fin grid\n");
}

int busv(float *x, float *y, float *z, int ic, float rlbox, int ngrid, int nobj,
         int *lirst, int *ll, int *grupo, float r0, int iper, int *listvec, 
         int *nvec)
{
  int ixc, iyc, izc;
  int ixci, iyci, izci;
  int ixcf, iycf, izcf;
  int ix, iy, iz;
  int ixx, iyy, izz;
  int ibox;
  float xx, yy, zz, dis;
  int i;
  float rlbox2;

  rlbox2=rlbox/2.0;
  ixc=(int)(x[ic]/rlbox*ngrid);
  ixci=ixc-1;
  ixcf=ixc+1;
  iyc=(int)(y[ic]/rlbox*ngrid);
  iyci=iyc-1;
  iycf=iyc+1;
  izc=(int)(z[ic]/rlbox*ngrid);
  izci=izc-1;
  izcf=izc+1;
  if ( iper == 0 ) /*no periodico*/
  {
    if( ixci < 0 ) ixci = 0;
    if( iyci < 0 ) iyci = 0;
    if( izci < 0 ) izci = 0;
    if( ixcf >= ngrid ) ixcf = ngrid-1;
    if( iycf >= ngrid ) iycf = ngrid-1;
    if( izcf >= ngrid ) izcf = ngrid-1;
  } 
  else if ( iper != 1 )
  {
    printf("la periodicidad debe ser 0 o 1\n");
    exit(0);
  }
  for( ixx = ixci ; ixx <= ixcf ; ixx++)
  {
    ix=ixx;
    if(iper == 1)
    {
      if(ix >= ngrid) ix = ix - ngrid;
      if(ix < 0) ix = ix + ngrid;
    }
    for( iyy = iyci ; iyy <= iycf ; iyy++)
    {
      iy=iyy;
      if(iper == 1)
      {
        if(iy >= ngrid) iy = iy - ngrid;
        if(iy < 0) iy = iy + ngrid;
      }
      for( izz = izci ; izz <= izcf ; izz++)
      {
        iz=izz;
        if(iper == 1)
        {
          if(iz >= ngrid) iz = iz - ngrid;
          if(iz < 0) iz = iz + ngrid;
        }

        ibox=(ix * ngrid + iy) * ngrid + iz ;
        i=lirst[ibox];
        if(i==-1)continue;
        do
        {
          i=ll[i];
          xx=x[i]-x[ic];
          yy=y[i]-y[ic];
          zz=z[i]-z[ic];
          if( iper == 1 )
          {
            if( xx > rlbox2 ) xx = xx - rlbox;
            if( yy > rlbox2 ) yy = yy - rlbox;
            if( zz > rlbox2 ) zz = zz - rlbox;
            if( xx < -rlbox2 ) xx = xx + rlbox;
            if( yy < -rlbox2 ) yy = yy + rlbox;
            if( zz < -rlbox2 ) zz = zz + rlbox;
          }
          dis=sqrt(xx*xx+yy*yy+zz*zz);
          if( dis < r0 && grupo[i] == 0 && i != ic )
          {
            listvec[*nvec]=i;
            *nvec=*nvec+1;
          }
        } while( i != lirst[ibox] ); /*fin laso particulas del grid*/
      } /*fin izz*/
    } /*fin iyy*/
  } /*fin ixx*/
}
